//卡尔曼滤波
#ifndef HITSZ_DIP_PRACTICUM_KMF_H
#define HITSZ_DIP_PRACTICUM_KMF_H

#include <Eigen/Dense>
#include <vector>
#include <iostream>

namespace dip
{
    class KalmanFilter
    {
        public:
            KalmanFilter(){};

            /**
             * @brief 指定公式的初始化
             * @brief formula: x_k+1 = Fx_k + Bu_k;z_k = Hx_k,x为n维，z为m维
             * @param _F,状态转移矩阵nxn
             * @param _B,n x u
             * @param _H,m x n
             */
            KalmanFilter(const Eigen::MatrixXf &_F,const Eigen::MatrixXf &_B,const Eigen::MatrixXf &_H):
                F(_F),B(_B),H(_H)
            { //cov根据定义为cov(xk - xk|k-1),因此为n x n矩阵,n为x的维度,即F的维度
                this->dim_x = F.cols();
                this->dim_z = H.rows();
                this->initializeCov();
                // this->isinitialized = true;
            };
            ~KalmanFilter(){};
            /**
             * @brief 初始化协方差矩阵
             */ 
            void initializeCov()
            {
                //如果不知道状态变量维度
                if (this->dim_x < 0){return;}
                // this->Cov.resize(this->dim_x,this->dim_x);
                this->Cov = Eigen::MatrixXf::Identity(this->dim_x,this->dim_x);//初始化为单位阵
            }

            /**
             * @brief 判断是否初始化成功
             * @note 由于B并不是必须的，所以这里只判断了维度和F/H/R/Q/Cov
             * @note 而X/Y/Z/K都是计算得到的，没有进行判断
             */ 
            bool assert_initialized()
            {
                if (this->dim_x < 0 || this->dim_z <0){return false;}
                else if (F.cols() ==0 || F.rows() == 0){return false;}
                else if (H.cols() ==0 || H.rows() == 0){return false;}
                else if (R.cols() ==0 || R.rows() == 0){return false;}
                else if (Q.cols() ==0 || Q.rows() == 0){return false;}
                else if (Cov.cols() ==0 || Cov.rows() == 0){return false;}

                this->isinitialized = true;
                return true;
            }
            //set/get
            void setF(const Eigen::MatrixXf &_F){this->F = _F;this->dim_x = _F.cols();this->initializeCov();}
            void setQ(const Eigen::MatrixXf &_Q){this->Q = _Q;}
            void setR(const Eigen::MatrixXf &_R){this->R = _R;}
            void setB(const Eigen::MatrixXf &_B){this->B = _B;}
            void setH(const Eigen::MatrixXf &_H){this->H = _H;this->dim_z = _H.rows();}
            void setState(const Eigen::VectorXf &_x){this->X = _x;}//这个接口用于实现对内部状态的限幅

            void getState(Eigen::VectorXf &_x){_x = this->X;}
            void getCov(Eigen::MatrixXf &_cov){_cov = this->Cov;}

            //update/predict
            /**
             * @brief 重置，这里只将协方差矩阵重新初始化
             * @param _x,新的状态变量值
             */ 
            void reset(const Eigen::VectorXf &_x)
            {
                if (!this->assert_initialized()){return ;}
                if (_x.rows() != this->dim_x){
                    std::cerr<<"[Kalman Filter] RESET Failed:required"<<this->dim_x<<","<<_x.rows()<<std::endl;
                    return;
                }
                this->X = _x;
                this->initializeCov();
            }

            /**
             * @brief 根据测量更新协方差矩阵
             * @param _z,测量值,mx1
             */ 
            void update(const Eigen::VectorXf &_z)
            {
                if (!this->assert_initialized()){return ;}
                if (_z.rows() != this->dim_z){
                    std::cerr<<"[Kalman Filter]:Required dim:"<<this->dim_z<<",get:"<<_z.cols()<<std::endl;
                    throw(1);}
                Z = _z;
                Y = Z - H*X;
                Eigen::MatrixXf S = H*Cov*H.transpose() + R;
                K = Cov*H.transpose()*S.inverse();//计算kalman增益
                X += K*Y;//不知道能不能这样写
                Cov = (Eigen::MatrixXf::Identity(this->dim_x,this->dim_x) - K*H)*Cov;
            }

            /**
             * @brief 预测,输出先验值
             * @param _u,控制量
             * @param _x,输出状态变量
             */ 
            bool predict(const Eigen::VectorXf &_u,Eigen::VectorXf &_x)
            {
                if (!this->assert_initialized()){return false;}
                if (X.cols() ==0 || X.rows() == 0){return false;}
                // this->print_debug_string();
                U = _u;
                // std::cout<<X<<std::endl;
                X = F*X + B*U;
                // std::cout<<X<<std::endl;
                Cov = F*Cov*F.transpose() + Q;
                _x = X;//先验
                return true;
            }

            /**
             * @brief 无控制量的预测
             * @param _x
             */ 
            bool predict(Eigen::VectorXf &_x)
            {
                if (!this->assert_initialized()){return false;}
                if (X.cols() ==0 || X.rows() == 0){return false;}
                X = F*X;
                Cov = F*Cov*F.transpose() + Q;
                _x = X;//先验
                return true;
            }

            //visualize
            void print_debug_string()
            {
                if (!this->isinitialized)
                {
                    std::cout<<"[FILTER ERROR]:filter is not initialized"<<std::endl;
                    return ;
                }
                std::cout<<"Kalman Filter Initialized as :"<<std::endl;
                std::cout<<"\t n="<<this->dim_x<<",m="<<this->dim_z<<std::endl;
                std::cout<<"\t F="<<this->F<<std::endl;
                if (this->B.rows() >0 || this->B.cols()>0){std::cout<<"\t B="<<this->B<<std::endl;}
                std::cout<<"\t H="<<this->H<<std::endl;
                std::cout<<"\t R="<<this->R<<std::endl;
                std::cout<<"\t Q="<<this->Q<<std::endl;
            }

        public:
            bool isinitialized = false;
            //是否初始化成功,需要同时初始化所有矩阵 + 状态变量维度
        private:
            //formula: X_k+1 = FX_k + BU_k + w_k;z_k = Hx_k + v_k
            //state variable
            int dim_x = -1;//x的维数,n
            int dim_z = -1;//测量空间维度,m
            Eigen::VectorXf X;//状态变量
            Eigen::VectorXf U;//控制量
            Eigen::VectorXf Z;//观测量
            Eigen::VectorXf Y;//y = z - hx
            Eigen::MatrixXf Cov;//协方差矩阵

            //noise(param)
            Eigen::MatrixXf Q;//测量噪声协方差矩阵
            Eigen::MatrixXf R;//观测噪声协方差矩阵

            //matrix
            Eigen::MatrixXf F;
            Eigen::MatrixXf B;
            Eigen::MatrixXf H;
            Eigen::MatrixXf K;//kalman gain,n x m

    };
}
#endif